#include <Motor.h>

Motor::Motor()
{}

void Motor::forward()
{
    leftMotor.write(getPulse(180));
    rightMotor.write(getPulse(0));
}

void Motor::reverse()
{
    leftMotor.write(getPulse(0));
    rightMotor.write(getPulse(180));
}

void Motor::rightReverse()
{
    rightMotor.write(getPulse(180));
}

void Motor::rightForward()
{
    rightMotor.write(getPulse(0));
}

void Motor::leftReverse()
{
    leftMotor.write(getPulse(0));
}

void Motor::leftForward()
{
    leftMotor.write(getPulse(180));
}

void Motor::turnLeft()
{
    leftReverse();
    rightForward();
    rightForward();
}

void Motor::turnLeft(int time)
{
    leftReverse();
    rightForward();
    delay(time);
    rightForward();
}
        
void Motor::turnRight()
{
    rightReverse();
    leftForward();
    leftForward();
}

void Motor::turnRight(int time)
{
    rightReverse();
    leftForward();
    delay(time);
    leftForward();
}

void Motor::stop()
{
    leftMotor.write(getPulse(100));
    rightMotor.write(getPulse(100));
}

int Motor::getPulse(int angle)
{
  int pulse;
  pulse = map(angle, 0, 180, 544, 2400);
  
  return pulse;
}

void Motor::rightInit(int pin)
{
    rightMotor.attach(pin);
}

void Motor::leftInit(int pin)
{
    leftMotor.attach(pin);
}

void Motor::setSpeed(int val, char side)
{
    switch(side)
    {
        case 'l':
            leftMotor.write(getPulse(val));
            break;
        case 'r':
            rightMotor.write(getPulse(val));
            break;
    }
}